#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <assert.h>
#include <string.h>
#include <time.h>

#define RTP_VESION              2

#define RTP_PAYLOAD_TYPE_H264   96
#define RTP_PAYLOAD_TYPE_AAC    97

#define RTP_HEADER_SIZE         12
#define RTP_MAX_PKT_SIZE        1400
#define SERVER_HOST "172.22.188.105"
#define FILE_NAME "d://cwork//test.h264"
#define SERVER_RTP_PORT  12034
#define SERVER_RTCP_PORT 12035
struct RtpHeader
{
    /* byte0 */
    uint8_t csrcLen: 4;
    uint8_t extension: 1;
    uint8_t padding: 1;
    uint8_t version: 2;

    /* byte1 */
    uint8_t payloadType: 7;
    uint8_t marker: 1;

    /* bytes 2,3 */
    uint16_t seq;

    /* bytes 4-7 */
    uint32_t timestamp;

    /* bytes 8-11 */
    uint32_t ssrc;
};

struct RtpPacket
{
    struct RtpHeader rtpHeader;
    uint8_t payload[0];
};

static inline int startCode3(char* buf)
{
    if(buf[0] == 0 && buf[1] == 0 && buf[2] == 1)
        return 1;
    else
        return 0;
}

static inline int startCode4(char* buf)
{
    if(buf[0] == 0 && buf[1] == 0 && buf[2] == 0 && buf[3] == 1)
        return 1;
    else
        return 0;
}

static char* findNextStartCode(char* buf, int len)
{
    int i;
    if(len < 3)
        return NULL;
    
    for(i = 0; i < len - 3; ++i)
    {
        if(startCode3(buf) || startCode4(buf))
            return buf;
        ++buf;
    }

    if(startCode3(buf))
        return buf;

    return NULL;
}

static int getFrameFromH264File(int fd, char* frame, int size)
{
    int rSize, frameSize;
    char* nextStartCode;

    if(fd < 0)
        return fd;
    rSize = read(fd, frame, size);
    if(!startCode3(frame) && !startCode4(frame))
        return -1;

    nextStartCode = findNextStartCode(frame + 3, rSize - 3);
    if(!nextStartCode)
    {
        //lseek(fd, 0, SEEK_SET);
        //frameSize = rSize;
        return -1;
    }
    else {
        frameSize = (nextStartCode - frame);
        lseek(fd, frameSize - rSize, SEEK_CUR);
    }
    
    return frameSize;
}

void rtpHeaderInit(struct RtpPacket* rtpPacket, uint8_t csrcLen, uint8_t extension,
    uint8_t padding, uint8_t version, uint8_t payloadType, uint8_t marker,
    uint16_t seq, uint32_t timestamp, uint32_t ssrc){
    
    rtpPacket->rtpHeader.csrcLen = csrcLen;
    rtpPacket->rtpHeader.extension = extension;
    rtpPacket->rtpHeader.padding = padding;
    rtpPacket->rtpHeader.version = version;
    rtpPacket->rtpHeader.payloadType = payloadType;
    rtpPacket->rtpHeader.marker = marker;
    rtpPacket->rtpHeader.seq = seq;
    rtpPacket->rtpHeader.timestamp = timestamp;
    rtpPacket->rtpHeader.ssrc = ssrc;
}

int rtpSendPacket(int socket, const char* ip, int16_t port, struct RtpPacket* rtpPacket,
    uint32_t dataSize)
{
    struct sockaddr_in addr;
    int ret;

    addr.sin_family = AF_INET;
    addr.sin_port = htons(port);
    addr.sin_addr.s_addr = inet_addr(ip);

    rtpPacket->rtpHeader.seq = htons(rtpPacket->rtpHeader.seq);
    rtpPacket->rtpHeader.timestamp = htonl(rtpPacket->rtpHeader.timestamp);
    rtpPacket->rtpHeader.ssrc = htonl(rtpPacket->rtpHeader.ssrc);

    ret = sendto(socket, (void*)rtpPacket, dataSize+RTP_HEADER_SIZE, 0,
                    (struct sockaddr*)&addr, sizeof(addr));

    rtpPacket->rtpHeader.seq = ntohs(rtpPacket->rtpHeader.seq);
    rtpPacket->rtpHeader.timestamp = ntohl(rtpPacket->rtpHeader.timestamp);
    rtpPacket->rtpHeader.ssrc = ntohl(rtpPacket->rtpHeader.ssrc);

    return ret;
}


static int rtpSendH264Frame(int socket, const char* ip, int16_t port,
    struct RtpPacket* rtpPacket, uint8_t* frame, uint32_t frameSize)
{
    uint8_t naluType; //nalu第一个字节
    int sendBytes = 0;
    int ret;

    naluType = frame[0];
    rtpPacket->rtpHeader.marker = 0;
    // nalu长度小于最大包长： 单一NALU单元模式
    if(frameSize <= RTP_MAX_PKT_SIZE){
        /*
         *   0 1 2 3 4 5 6 7 8 9
         *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
         *  |F|NRI|  Type   | a single NAL unit ... |
         *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
         */
        memcpy(rtpPacket->payload, frame, frameSize);
        ret = rtpSendPacket(socket, ip, port, rtpPacket, frameSize);
        if(ret < 0)
            return -1;

        rtpPacket->rtpHeader.seq++;
        sendBytes += ret;
        if ((naluType & 0x1F) == 7 || (naluType & 0x1F) == 8) // 如果是SPS、PPS就不需要加时间戳
            goto out;
    }
    else // nalu长度小于最大包长：分片模式
    {
        /*
         *  0                   1                   2
         *  0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3
         * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
         * | FU indicator  |   FU header   |   FU payload   ...  |
         * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
         */

        /*
         *     FU Indicator
         *    0 1 2 3 4 5 6 7
         *   +-+-+-+-+-+-+-+-+
         *   |F|NRI|  Type   |
         *   +---------------+
         */

        /*
         *      FU Header
         *    0 1 2 3 4 5 6 7
         *   +-+-+-+-+-+-+-+-+
         *   |S|E|R|  Type   |
         *   +---------------+
         */

        int pktNum = frameSize / RTP_MAX_PKT_SIZE;       // 有几个完整的包
        int remainPktSize = frameSize % RTP_MAX_PKT_SIZE; // 剩余不完整包的大小
        int i, pos = 1;

        /* 发送完整的包 */
        for (i = 0; i < pktNum; i++)
        {
            rtpPacket->payload[0] = (naluType & 0x60) | 28;
            rtpPacket->payload[1] = naluType & 0x1F;
            if (i == 0) //第一包数据
                rtpPacket->payload[1] |= 0x80; // start
            else if (remainPktSize == 0 && i == pktNum - 1){ //最后一包数据
                rtpPacket->rtpHeader.marker = 1;
                rtpPacket->payload[1] |= 0x40; // end
            }

            memcpy(rtpPacket->payload+2, frame+pos, RTP_MAX_PKT_SIZE);
            ret = rtpSendPacket(socket, ip, port, rtpPacket, RTP_MAX_PKT_SIZE+2);
            if(ret < 0)
                return -1;

            rtpPacket->rtpHeader.seq++;
            sendBytes += ret;
            pos += RTP_MAX_PKT_SIZE;
        }

        /* 发送剩余的数据 */
        if (remainPktSize > 0)
        {
            rtpPacket->payload[0] = (naluType & 0x60) | 28;
            rtpPacket->payload[1] = naluType & 0x1F;
            rtpPacket->payload[1] |= 0x40; //end
            rtpPacket->rtpHeader.marker = 1;
            memcpy(rtpPacket->payload+2, frame+pos, remainPktSize+2);
            ret = rtpSendPacket(socket, ip, port, rtpPacket, remainPktSize+2);
            if(ret < 0)
                return -1;

            rtpPacket->rtpHeader.seq++;
            sendBytes += ret;
        }
    }

out:

    return sendBytes;
}

int main(int argc, int argv[])
{
    int socket_desc;
    struct sockaddr_in server;
    char sdp[16384];
    char message[2000];
    char server_reply[2000];

    //Create socket
    socket_desc = socket(AF_INET, SOCK_STREAM, 0);
    if(socket_desc == -1)
    {
        printf("Could not create socket");
    }

    server.sin_addr.s_addr = inet_addr(SERVER_HOST);
    server.sin_family = AF_INET;
    server.sin_port = htons(554);

    //Connect to remote server
    if(connect(socket_desc, (struct sockaddr*)&server, sizeof(server)) < 0)
    {
        printf("connect error");
        return 1;
    }

    sprintf(message,
        "OPTIONS rtsp://%s:%d/%s RTSP/1.0\r\n"
        "CSeq: %d\r\n\r\n",
        SERVER_HOST,
        554,
        "test3",
        1);

    if(send(socket_desc, message, strlen(message), 0) < 0){
        printf("Send failed\n");
        return 1;
    }

    printf("Data Send\n");

    if(recv(socket_desc, server_reply, sizeof(server_reply), 0) < 0){
        printf("recv failed\n");
        return 1;
    }

    printf("Reply received\n");
    printf(server_reply);

    sprintf(sdp,
        "v=0\r\n"
        "o=- 9%ld 1 IN IP4 %s\r\n"
        "c=IN IP4 %s\r\n"
        "t=0 0\r\n"
        "a=control:*\r\n"
        "m=video %d RTP/AVP 96\r\n"
        "a=rtpmap:96 H264/90000\r\n"
        "a=framerate:25\r\n"
       // "a=fmtp:96 profile-level-id=64001F; packetization-mode=1; sprop-parameter-sets=Z2QAH6zZQPA1+X/wEQAREQAAAwABAAADADwPGDGW,aOvjyyLA\r\n"
        "a=control:streamid=0\r\n",
        time(NULL),
        SERVER_HOST,
        SERVER_HOST,
        0,
        0);

    int sdplen = strlen(sdp);
    
    sprintf(message,
        "ANNOUNCE rtsp://%s:%d/%s RTSP/1.0\r\n"
        "Content-Type: application/sdp\r\n"
        "CSeq: %u\r\n"
        "Content-Length: %d\r\n"
        "\r\n"
        "%s",
        SERVER_HOST,
        554,
        "test3",
        2,
        sdplen,
        sdp);
    if(send(socket_desc, message, strlen(message), 0) < 0){
        printf("Send failed\n");
        return 1;
    }

    printf("Data Send ANNOUNCE\n");
    memset(&server_reply, 0, sizeof(server_reply));
    if(recv(socket_desc, server_reply, sizeof(server_reply), 0) < 0){
        printf("recv failed");
        return 1;
    }

    printf("Reply received\n");
    puts(server_reply);

    sprintf(message,
        "SETUP rtsp://%s:%d/%s/streamid=0 RTSP/1.0\r\n"
        "Transport: RTP/AVP/UDP;unicast;client_port=%d-%d;mode=record\r\n"
        "CSeq: %u\r\n"
        "\r\n",
        SERVER_HOST,
        554,
        "test3",
        SERVER_RTP_PORT,
        SERVER_RTCP_PORT,
        3);

    if(send(socket_desc, message, strlen(message), 0) < 0){
        printf("Send failed\n");
        return 1;
    }

    printf("Data Send SETUP\n");

    memset(&server_reply, 0, sizeof(server_reply));
    if(recv(socket_desc, server_reply, sizeof(server_reply), 0) < 0){
        printf("recv failed\n");
        return 1;
    }

    uint16_t server_rtp_port;
    uint16_t server_rtcp_port; 
    sscanf(server_reply, "%*[^;];%*[^;];%*[^;];%*[^=]=%hu-%hu", &server_rtp_port, &server_rtcp_port);
    printf("Reply received %hu %hu\n", server_rtp_port, server_rtcp_port);
    puts(server_reply);

    sprintf(message,
        "RECORD rtsp://%s:%d/%s RTSP/1.0\r\n"
        "Range: npt=0.000-\r\n"
        "CSeq: %u\r\n"
        "\r\n",
        SERVER_HOST,
        554,
        "test3",
        4);

    if(send(socket_desc, message, strlen(message), 0) < 0){
        printf("Send failed\n");
        return 1;
    }

    printf("Data Send RECORD\n");
    memset(&server_reply, 0, sizeof(server_reply));
    if(recv(socket_desc, server_reply, sizeof(server_reply), 0) < 0){
        printf("recv failed\n");
        return 1;
    }

    printf("Reply received\n");
    printf(server_reply);

    int socket_rtp;
    socket_rtp = socket(AF_INET, SOCK_DGRAM, 0);
    if(socket_rtp == -1){
        printf("create rtp socket err\n");
        return 1;
    }
    
    int on = 1;
    setsockopt(socket_rtp, SOL_SOCKET, SO_REUSEADDR, (const char*)&on, sizeof(on));
    struct sockaddr_in addr;
    addr.sin_family = AF_INET;
    addr.sin_port = htons(SERVER_RTP_PORT);
    addr.sin_addr.s_addr = inet_addr("0.0.0.0");
    if(bind(socket_rtp, (struct sockaddr*)&addr, sizeof(struct sockaddr)) < 0){
        return -1;
    }

    int socket_rtcp;
    socket_rtcp = socket(AF_INET, SOCK_DGRAM, 0);
    if(socket_rtcp == -1){
        printf("create rtp socket err\n");
        return 1;
    }
    
    int on1 = 1;
    setsockopt(socket_rtcp, SOL_SOCKET, SO_REUSEADDR, (const char*)&on1, sizeof(on1));
    struct sockaddr_in addr1;
    addr1.sin_family = AF_INET;
    addr1.sin_port = htons(SERVER_RTCP_PORT);
    addr1.sin_addr.s_addr = inet_addr("0.0.0.0");
    if(bind(socket_rtcp, (struct sockaddr*)&addr1, sizeof(struct sockaddr)) < 0){
        return -1;
    }

    int frameSize, startCode;
    char* frame = malloc(500000);
    struct RtpPacket* rtpPacket = (struct RtpPacket*)malloc(500000);
    int fd = open(FILE_NAME, O_RDONLY);
    assert(fd > 0);
    rtpHeaderInit(rtpPacket, 0, 0, 0, RTP_VESION, RTP_PAYLOAD_TYPE_H264, 0,
                    0, 0, 0x88923423);

    printf("start play\n");
    printf("server port:%d\n", server_rtp_port);

    while(1){
        frameSize = getFrameFromH264File(fd, frame, 500000);
        if(frameSize < 0)
        {
            break;
        }

        if(startCode3(frame))
            startCode = 3;
        else
            startCode = 4;

        frameSize -= startCode;
        rtpSendH264Frame(socket_rtp, SERVER_HOST, server_rtp_port,
            rtpPacket, frame + startCode, frameSize);
        rtpPacket->rtpHeader.timestamp += 90000/25;
        usleep(1000 * 1000 / 25);
    }

    free(frame);
    free(rtpPacket);

    sprintf(message,
        "TEARDOWN rtsp://%s:%d/%s RTSP/1.0\r\n"
        "CSeq: %u\r\n"
        "\r\n",
        SERVER_HOST,
        554,
        "test3",
        5);

    if(send(socket_desc, message, strlen(message), 0) < 0){
        printf("Send failed\n");
        return 1;
    }

    printf("Data Send TEARDOWN\n");
    memset(&server_reply, 0, sizeof(server_reply));
    if(recv(socket_desc, server_reply, sizeof(server_reply), 0) < 0){
        printf("recv failed\n");
        return 1;
    }

    printf("Reply received\n");
    printf(server_reply);

    return 0;
}
